#include "CaP_Validator.hpp"

void CaP_Validator::resetSimulation(dReal angolo)
{
	dReal yPos, zPos;

	dBodyDestroy(boxBody);
	dBodyDestroy(sferaBody);

	boxBody = dBodyCreate(world);
	dBodySetMass(boxBody,&boxMass);
	dGeomSetBody(boxGeom, boxBody);
	dBodySetPosition(boxBody, 0, 0, 0.05);
	dBodySetLinearVel(boxBody, 0, 0, 0);
	dBodySetAngularVel(boxBody, 0, 0, 0);

	yPos = cos(angolo * CAP_VALIDATOR_PI / 180.0) * DIM_ASTA;
	zPos = sin(angolo * CAP_VALIDATOR_PI / 180.0) * DIM_ASTA;

	sferaBody = dBodyCreate (world);
    dBodySetMass (sferaBody,&sferaMass);
    dGeomSetBody (sferaGeom,sferaBody);
	dBodySetPosition(sferaBody, 0, yPos, zPos);
	dBodySetLinearVel(sferaBody, 0, 0, 0);
	dBodySetAngularVel(sferaBody, 0, 0, 0);

	dJointAttach(cerniera1, sferaBody, boxBody);
	dJointSetHingeAnchor(cerniera1, 0, 0, 0.05);

	dJointAttach(motore, boxBody, 0);
	dJointSetSliderAxis (motore, 0,1,0);
	dJointSetSliderParam(motore, dParamVel, 0);
	dJointSetSliderParam(motore, dParamFMax, 200);

}

void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
	CaP_Validator *cVal = (CaP_Validator*)data;
    dBodyID b1 = dGeomGetBody(o1);
    dBodyID b2 = dGeomGetBody(o2);
    dContact contact;
    contact.surface.mode = dContactBounce | dContactSoftCFM;
    // friction parameter
    contact.surface.mu = 0;
    // bounce is the amount of "bouncyness".
    contact.surface.bounce = 0.9;
    // bounce_vel is the minimum incoming velocity to cause a bounce
    contact.surface.bounce_vel = 0.1;
    // constraint force mixing parameter
    contact.surface.soft_cfm = 0.001;
    if ( dCollide (o1,o2,1,&contact.geom,sizeof(dContact))) {
        dJointID c = dJointCreateContact (cVal->world,cVal->contactgroup,&contact);
        dJointAttach (c,b1,b2);
    }
}


dReal CaP_Validator::PID(dReal angolo)
{
	dReal errSig, P_term, D_term, I_term;

	//Controllore PID tarato "ad occhio"...
	dReal P_const = 1.5;
	dReal I_const = 0.03;
	dReal D_const = 0.05;

	errSig = 90-angolo;

	P_term = P_const * errSig;

    D_term = D_const * ( errSig - D_state);
    D_state = errSig;

    I_state = I_state + errSig;

	//Eliminazione wind-up...
    if (I_state > I_STATE_MAX)
        I_state = I_STATE_MAX;
    else
		if (I_state < I_STATE_MIN)
			I_state = I_STATE_MIN;
    I_term = I_state * I_const;

    return  P_term + I_term + D_term;

}

dReal CaP_Validator::simLoop (int pause)
{
    //static int reset = 0;
	const dReal *pos,*pos2;
    //const dReal *R;
	//const dReal *vel;
	dReal angolo, controlSignal;

	pos = dGeomGetPosition (sferaGeom);
	pos2 = dGeomGetPosition (boxGeom);
	angolo = (dReal)atan2(pos[2]-pos2[2],pos[1]-pos2[1]);
	angolo = angolo * 180 / CAP_VALIDATOR_PI;
	//printf("%f\n",angolo);


	//else
	controlSignal = PID(angolo);
	//cs2 = controlloPOS(pos2[1]);
	//controlSignal += cs2;
	dJointSetSliderParam(motore, dParamVel, controlSignal);

    dSpaceCollide (space,(void*)this,&nearCallback);
    // step the simulation
    dWorldQuickStep (world,0.01);
	simTime+=0.01;
    // remove all contact joints
    dJointGroupEmpty (contactgroup);
    // redraw sphere at new location
    pos = dGeomGetPosition (sferaGeom);
	printf("%f\t%f\t%f\t%f\t",simTime,pos[0],pos[1],pos[2]);



	//vel = dBodyGetLinearVel(body);
	//printf("%f\t%f\t%f\n",vel[0],vel[1],vel[2]);
    pos2 = dGeomGetPosition (boxGeom);
	angolo = (dReal)atan2(pos[2]-pos2[2],pos[1]-pos2[1]);
	angolo = angolo * 180 / CAP_VALIDATOR_PI;
	printf("%f\t%f\t%f\t%f\t%f\n",pos2[0],pos2[1],pos2[2],angolo,controlSignal);
	//printf("\n");
	//R = dGeomGetRotation (geom);
    //printf("%f\t%f\t%f\n",R[0],R[1],R[2]);

	//dsDrawSphere (pos,R,dGeomSpheMreGetRadius (geom));
	return angolo;
}

CaP_Validator::CaP_Validator(GAmodSettings &settings,error_hand_type eht) throw(CaP_ValidatorException)
{
	//const GAmodStrSetting *strsett;
	TemplateGAmodObjWrapper<string> *strsett;
    const char *tmp;

	//strsett = (GAmodStrSetting*) settings.get_setting("simSteps");
    //tmp = (strsett) ? ((GAmodStrSetting*)strsett)->toStr() : NULL;

    strsett = (TemplateGAmodObjWrapper<string>*)settings.get_setting("simSteps");
    tmp = (strsett) ? strsett->value.c_str() : NULL;

    if(tmp)
	{
		 simSteps = atoi(tmp);
        //free((void*)tmp);
        IF_DBG_VERB(printf("[INFO] CaP_Validator::CaP_Validator: simSteps <%s> = %d\n",tmp,simSteps);)

		if(simSteps<=0)
			throw CaP_ValidatorException("CaP_Validator::CaP_Validator: valore di simSteps errato");
	}
	else
	{
		IF_DBG_VERB(printf("[WARN] CaP_Validator::CaP_Validator: simSteps non specificato, assumo valore stardard (500)\n");)
		simSteps = 500;
	}

	//strsett = (GAmodStrSetting*) settings.get_setting("minStartAngle");
    //tmp = (strsett) ? ((GAmodStrSetting*)strsett)->toStr() : NULL;

    strsett = (TemplateGAmodObjWrapper<string>*)settings.get_setting("minStartAngle");
    tmp = (strsett) ? strsett->value.c_str() : NULL;

    if(tmp)
	{
		 minStartAngle = atof(tmp);
        //free((void*)tmp);
        IF_DBG_VERB(printf("[INFO] CaP_Validator::CaP_Validator: minStartAngle <%s> = %f\n",tmp,minStartAngle);)

		if(minStartAngle<=0 || minStartAngle >=45)
			throw CaP_ValidatorException("CaP_Validator::CaP_Validator: valore di minStartAngle errato");
	}
	else
	{
		IF_DBG_VERB(printf("[WARN] CaP_Validator::CaP_Validator: minStartAngle non specificato, assumo valore stardard (5)\n");)
		minStartAngle = 5.0;
	}

	//strsett = (GAmodStrSetting*) settings.get_setting("maxStartAngle");
    //tmp = (strsett) ? ((GAmodStrSetting*)strsett)->toStr() : NULL;

    strsett = (TemplateGAmodObjWrapper<string>*)settings.get_setting("maxStartAngle");
    tmp = (strsett) ? strsett->value.c_str() : NULL;

    if(tmp)
	{
		maxStartAngle = atof(tmp);
        //free((void*)tmp);
        IF_DBG_VERB(printf("[INFO] CaP_Validator::CaP_Validator: maxStartAngle <%s> = %f\n",tmp,maxStartAngle);)

		if(maxStartAngle<=0 || maxStartAngle >=45 || maxStartAngle<minStartAngle)
			throw CaP_ValidatorException("CaP_Validator::CaP_Validator: valore di maxStartAngle errato");
	}
	else
	{
		IF_DBG_VERB(printf("[WARN] CaP_Validator::CaP_Validator: maxStartAngle non specificato, assumo valore stardard (15)\n");)
		maxStartAngle = 15.0;
	}

	//strsett = (GAmodStrSetting*) settings.get_setting("n_eval");
    //tmp = (strsett) ? ((GAmodStrSetting*)strsett)->toStr() : NULL;
    strsett = (TemplateGAmodObjWrapper<string>*)settings.get_setting("n_eval");
    tmp = (strsett) ? strsett->value.c_str() : NULL;
    if(tmp)
	{
		 n_eval = atoi(tmp);
        //free((void*)tmp);
        IF_DBG_VERB(printf("[INFO] CaP_Validator::CaP_Validator: n_evals <%s> = %d\n",tmp,n_eval);)

		if(n_eval<=0)
			throw CaP_ValidatorException("CaP_Validator::CaP_Validator: valore di n_evals errato");
	}
	else
	{
		IF_DBG_VERB(printf("[WARN] CaP_Validator::CaP_Validator: n_eval non specificato, assumo valore stardard (1)\n");)
		n_eval = 1;
	}

	max_score = n_eval * simSteps;

	IF_DBG_VERB(printf("[INFO] CaP_Validator::CaP_Validator: max score =  %d \n", max_score);)

	IF_DBG_VERB(printf("[INFO] CaP_Validator::CaP_Validator: Inizializzazione ODE...");)
	dInitODE ();

    // create world
    world = dWorldCreate ();
    space = dHashSpaceCreate (0);
    dWorldSetGravity (world,0,0,-9.81);
    dWorldSetCFM (world,1e-5);
    dCreatePlane(space,0,0,1,0);
    contactgroup = dJointGroupCreate (0);
	//giunti = dJointGroupCreate(0);
    // create object
    sferaBody = dBodyCreate (world);
    sferaGeom = dCreateSphere (space,0.5);
    dMassSetSphereTotal (&sferaMass,1,0.5);
    dBodySetMass (sferaBody,&sferaMass);
    dGeomSetBody (sferaGeom,sferaBody);
    // set initial position
    dBodySetPosition (sferaBody,0,0,DIM_ASTA);

	boxBody = dBodyCreate(world);
	boxGeom = dCreateBox(space,0.5, 1, 0.1);
	dMassSetBoxTotal(&boxMass,3, 0.5, 1, 0.1);
	dBodySetMass(boxBody,&boxMass);
	dGeomSetBody(boxGeom, boxBody);
	dBodySetPosition(boxBody, 0, 0, 0.05);

	cerniera1 = dJointCreateHinge(world,0);
	dJointAttach(cerniera1, sferaBody, boxBody);
	dJointSetHingeAnchor(cerniera1, 0, 0, 0.05);

	motore = dJointCreateSlider(world,0);
	//dJointSetLMotorNumAxes(motore, 1);
	dJointAttach(motore, boxBody, 0);
	dJointSetSliderAxis (motore, 0,1,0);
	dJointSetSliderParam(motore, dParamVel, 0);
	dJointSetSliderParam(motore, dParamFMax, 200);

	simTime = 0;
	resetControllo();
	IF_DBG_VERB(printf("ok!\n");)
}

unsigned int CaP_Validator::valuta_pop(Popolazione *pop,stat_generation sg) throw(CaP_ValidatorException)
{
	//evpop=pop;
	//boost::thread* second_thread=new boost::thread(boost::bind(&CaP_Validator::valuta_pop_mt, this));
	//m_thread = boost::shared_ptr<boost::thread>(second_thread);
	try{
		return valuta_pop(pop,0,0,sg);
	}
	catch(CaP_ValidatorException &e)
	{
		throw e;
	}
	//second_thread->join();
}

unsigned int CaP_Validator::valuta_pop_mt()
{
try{
		//valuta_pop(evpop,51,0,Validator::STAT_ALL);
	}
	catch(CaP_ValidatorException &e)
	{
		//throw e;
		//do nothing for now
	}

}


unsigned int CaP_Validator::valuta_pop(Popolazione *pop, int eval_from, int eval_to,stat_generation sg) throw(CaP_ValidatorException)
{
	unsigned int 	n_ind,pop_size,score,evalN;
	clock_t 		t0,t1;
	unsigned int 	actEvals, bestScore;
	float 			evalTime;

	//char stringa[255];

	if(!pop)
		throw CaP_ValidatorException("CaP_Validator::valuta_pop: Popolazione non valida");

	pop_size = pop->get_size();
	if(eval_to == 0)
		eval_to = pop_size-1;


	actEvals = 0;
	t0 = clock();

	//printf("\n[");

	//sprintf(stringa,"");
	//printf("CaP_Validator::valuta_pop [%d,%d]\n",eval_from,eval_to);
	bestScore = 0;
	for(n_ind = eval_from; n_ind<=eval_to; n_ind++)
	{
		IF_DBG_VERB_DBG(printf("------INVIDIDUO %d-------\n",n_ind);)
		if((*pop)[n_ind].mod == 0)
		{
			//sprintf(stringa,"%s %d",stringa, (*pop)[n_ind].fitness);
			continue;
		}
		//VALIDAZIONE


		score = 0;
		for(evalN = 0 ; evalN<n_eval; evalN++)
		{
			actEvals++;
			(*pop)[n_ind].reset_FF();
			#ifdef INJECT_ERRORS
				(*pop)[n_ind].injectAllErrors();
			#endif
			score += valutaSingleRun((*pop)[n_ind]);
			IF_DBG_VERB_DBG(printf("Score: %d",score); getchar();)

		}

		(*pop)[n_ind].fitness = score;
		(*pop)[n_ind].mod = 0;
		if (score > bestScore) bestScore = score;
		//printf("%d ", score);
		//sprintf(stringa,"%s !%d",stringa,score);
	}

	//printf("]\n");

	//printf("[%s]\n",stringa);

	t1 = clock();
	evalTime = (t1-t0)/(float)(CLOCKS_PER_SEC);
	printf("[INFO] CaP_Validator::valuta_pop: EvT:[%f] NeV:[%d] SeT:[%f]\n",evalTime,actEvals,evalTime/actEvals);

	return bestScore;

}

#define OUT_CONTROL_BIT	4

HERAuint8 quantizzaAngolo(dReal errAngle)
{
	HERAuint8		inErr;
	unsigned int	absErr;
	//dReal			normErr;

	//errAngle = 31 * sqrt(errAngle/31.0);
	absErr = floor(ABS(errAngle));
	absErr = MIN(31,absErr);
	inErr = (HERAuint8) absErr;
	inErr&= 0x1F; //Inutile...
	if (errAngle<0)
		inErr |= 0x20;

	return inErr;
}

unsigned int CaP_Validator::valutaSingleRun(Individuo &Ind)
{
	unsigned int simLen, run, absErr, i, posErr;
	dReal startAngle, deltaAngle, angolo, controlSignal, errAngle;
	const dReal *pos,*pos2;
	HERAuint8 input[DEF_ROWS], result, inErr, psErr;


	simLen = 0;
	deltaAngle = minStartAngle + NORM_RAND * (maxStartAngle - minStartAngle);
	startAngle = (rand() % 2) ? 90-deltaAngle : 90+deltaAngle;

	IF_DBG_VERB_DBG(printf("[INFO]  CaP_Validator::valutaSingleRun: startAngle = %f\n",startAngle);)
	resetSimulation(startAngle);
	run = 1;

	memset(input, 0, sizeof(HERAuint8)*DEF_ROWS);

	while(run)
	{
		pos = dGeomGetPosition (sferaGeom);
		pos2 = dGeomGetPosition (boxGeom);
		angolo = (dReal)atan2(pos[2]-pos2[2],pos[1]-pos2[1]);
		angolo = angolo * 180 / CAP_VALIDATOR_PI;
		//printf("%f\n",angolo);

		errAngle = 90-angolo;

		/*absErr = ABS(floor(errAngle));
		absErr = MIN(31,absErr);
		inErr = (HERAuint8) absErr;
		inErr&= 0x1F; //Inutile...
		if (errAngle<0)
			inErr |= 0x20;
		*/

		inErr = quantizzaAngolo(errAngle);
		if (pos2[1]<0)
			inErr |= 1<<6;


		/*posErr = ABS(floor(0.5+pos[1]/7.0));
		psErr = (HERAuint8) posErr;
		if(pos[1]<0)
			psErr |= 0x02;
		inErr |= psErr << 7;
		*/
		//IF_DBG_VERB_DBG(printf("CaP_Validator::valutaSingleRun: posErr = %f, psErr = %d\n",pos[1],psErr);)
		IF_DBG_VERB_DBG(printf("CaP_Validator::valutaSingleRun: errAngle = %f\n",errAngle);)
		IF_DBG_VERB_DBG(printf("CaP_Validator::valutaSingleRun: inErr = %d[%X]\n",inErr,inErr);)
		for(i=0; i<DEF_ROWS; i++)
			if (inErr & (1<<i) )
				input[DEF_ROWS-i-1] = 1;
			else
				input[DEF_ROWS-i-1] = 0;
		result = (HERAuint8)Ind.simulate_run((op_type*)input,0) & ( (1<<(OUT_CONTROL_BIT)) - 1);
		IF_DBG_VERB_DBG(printf("CaP_Validator::valutaSingleRun: result = %d[%X]\n",result,result);)
		//else
		//controlSignal = controllo(angolo);
		controlSignal = MAX_CONTROL * (dReal)(result & ( (1<<(OUT_CONTROL_BIT-1)) - 1) ) / (float)((1<<(OUT_CONTROL_BIT-1)) - 1);
		//printf("<<<[%X]>>>\n", 1 << (OUT_CONTROL_BIT-1));
		if (result & ( 1 << (OUT_CONTROL_BIT-1) ) )
			controlSignal = -controlSignal;

		IF_DBG_VERB_DBG(printf("CaP_Validator::valutaSingleRun: cs = %f\n",controlSignal);)
		dJointSetSliderParam(motore, dParamVel, controlSignal);

		dSpaceCollide (space,(void*)this,&nearCallback);
		dWorldStep (world,0.01);

		simLen++;

		dJointGroupEmpty (contactgroup);

		pos = dGeomGetPosition (sferaGeom);
		pos2 = dGeomGetPosition (boxGeom);
		angolo = (dReal)atan2(pos[2]-pos2[2],pos[1]-pos2[1]);
		angolo = angolo * 180 / CAP_VALIDATOR_PI;
		//printf("%f\t%f\t%f\t%f\t%f\n",pos2[0],pos2[1],pos2[2],angolo,controlSignal);

		if(simLen>= simSteps)
			run = 0;
		if(angolo < 90-45 || angolo > 90+45)
			run = 0;
		if(pos2[1] >7 || pos2[1] <-7)
			run = 0;
		IF_DBG_VERB_DBG(getchar();)
	}

	return simLen;
}

unsigned int CaP_Validator::snapIndividuo(Individuo &Ind,char *path) throw(CaP_ValidatorException)
{
	unsigned int simLen, run, absErr, i, evalN, totSimLen;
	dReal startAngle, deltaAngle, angolo, controlSignal, errAngle;
	const dReal *pos,*pos2;
	HERAuint8 input[DEF_ROWS], result, inErr;
	FILE		*outFile;
	char		stringa[255];

	sprintf(stringa,"%s.txt",path);
	outFile =fopen(stringa, "w");

	if (!outFile)
		throw CaP_ValidatorException("CaP_Validator::snapIndividuo: impossibile aprire il file per il dump");

	totSimLen = 0;
	memset(input, 0, sizeof(HERAuint8)*DEF_ROWS);
	for(evalN = 0 ; evalN<n_eval; evalN++)
	{
		simLen = 0;
		Ind.reset_FF();
		//printf("*");

		deltaAngle = minStartAngle + NORM_RAND * (maxStartAngle - minStartAngle);
		startAngle = (rand() % 2) ? 90-deltaAngle : 90+deltaAngle;

		IF_DBG_VERB_DBG(printf("[INFO]  CaP_Validator::snapIndividuo: startAngle = %f\n",startAngle);)
		resetSimulation(startAngle);
		run = 1;



		while(run)
		{
			pos = dGeomGetPosition (sferaGeom);
			pos2 = dGeomGetPosition (boxGeom);
			angolo = (dReal)atan2(pos[2]-pos2[2],pos[1]-pos2[1]);
			angolo = angolo * 180 / CAP_VALIDATOR_PI;
			//printf("%f\n",angolo);

			errAngle = 90-angolo;
			/*absErr = ABS(floor(errAngle));
			absErr = MIN(31,absErr);
			inErr = (HERAuint8) absErr;
			inErr&= 0x1F;
			if (errAngle<0)
				inErr |= 0x20;*/

			inErr = quantizzaAngolo(errAngle);
			if (pos2[1]<0)
				inErr |= 1<<6;


			//printf("CaP_Validator::valutaSingleRun: errAngle = %f, absErr = %d\n",errAngle,absErr);
			//printf("CaP_Validator::valutaSingleRun: inErr = %d[%X]\n",inErr,inErr);
			for(i=0; i<DEF_ROWS; i++)
				if (inErr & (1<<i) )
					input[DEF_ROWS-i-1] = 1;
				else
					input[DEF_ROWS-i-1] = 0;
			result = (HERAuint8)Ind.simulate_run((op_type*)input,0) & ( (1<<(OUT_CONTROL_BIT)) - 1);
			controlSignal = MAX_CONTROL * (dReal)(result & ( (1<<(OUT_CONTROL_BIT-1)) - 1) ) / (float)((1<<(OUT_CONTROL_BIT-1)) - 1);
		//printf("<<<[%X]>>>\n", 1 << (OUT_CONTROL_BIT-1));
			if (result & ( 1 << (OUT_CONTROL_BIT-1) ) )
				controlSignal = -controlSignal;

			//printf("CaP_Validator::valutaSingleRun: cs = %f\n",controlSignal);
			dJointSetSliderParam(motore, dParamVel, controlSignal);

			dSpaceCollide (space,(void*)this,&nearCallback);
			dWorldStep (world,0.01);

			simLen++;
			totSimLen++;

			dJointGroupEmpty (contactgroup);

			pos = dGeomGetPosition (sferaGeom);
			pos2 = dGeomGetPosition (boxGeom);
			angolo = (dReal)atan2(pos[2]-pos2[2],pos[1]-pos2[1]);
			angolo = angolo * 180 / CAP_VALIDATOR_PI;
			fprintf(outFile,"%f\t%f\t%f\t%f\t",totSimLen*0.01,pos[0],pos[1],pos[2]);
			fprintf(outFile,"%f\t%f\t%f\t%f\t%f\n",pos2[0],pos2[1],pos2[2],angolo,controlSignal);

			if(simLen>= simSteps)
				run = 0;
			if(angolo < 90-45 || angolo > 90+45)
				run = 0;
			if(pos2[1] >7 || pos2[1] <-7)
				run = 0;
			//getchar();
		}
	}

	fclose(outFile);
	return simLen;
}

CaP_Validator::~CaP_Validator()
{
	IF_DBG_VERB(printf("CaP_Validator::~CaP_Validator: chiusura ODE...");)

	dJointGroupDestroy (contactgroup);
	dJointDestroy(cerniera1);
	dJointDestroy(motore);
    dSpaceDestroy (space);
    dWorldDestroy (world);
    dCloseODE();

    IF_DBG_VERB(printf("OK\n");)
}
